#include "Display.h"

void Display::skeletonData(pi_tracker::Skeleton::ConstPtr msg, int bonesIndex[2][15], IndList &LOI, float distances[15][15], float stDev[15], float vmean[15]){
  
  
  
}

void Display::drawSkeleton(pi_tracker::Skeleton::ConstPtr msg, int bonesIndex[2][15], IndList &LOI, float distances[15][15], float stDev[15], float vmean[15]) {
  int centerx = 316.7;
  int centery = 238.5 + 20;
  int fx = 524;
  int fy = 524;
  int mult = 1;
  
  using namespace cv;
  using namespace std;
  //std::cout << "12" << std::endl;
  //verificar tamanho
  //background = Mat::zeros(640+400, 480, CV_8UC3);
  background = Mat::zeros(480, 640 + 600, CV_8UC3);
  //camera = Mat::zeros(480, 640, CV_8UC3);
  //std::cout << "16" << std::endl;
  for (int i = 0; i < 15; i++) {
    
    Point st = Point(centerx + (fx * msg->position[bonesIndex[0][i]].x) / msg->position[bonesIndex[0][i]].z, centery + (-1) * (fy * msg->position[bonesIndex[0][i]].y) / msg->position[bonesIndex[0][i]].z);
    Point end = Point(centerx + (fx * msg->position[bonesIndex[1][i]].x) / msg->position[bonesIndex[1][i]].z, centery + (-1) * (fy * msg->position[bonesIndex[1][i]].y) / msg->position[bonesIndex[1][i]].z);
    line(camera, st, end, Scalar(0, 255, 0), 3, 8, 0);
    //std::cout << "22" << std::endl;
  }
  //std::cout << "24" << std::endl;
  
  //cv::Rect roi( cv::Point( 100, 100 ), skeletonMat.size() );
  //skeletonMat.copyTo( background( roi ) );
  
  std::vector<String> tabela1;
  
  std::ostringstream txs;
  String acBones[15] = {"HN ", "SS ", "TLS", "TRS", "TLH", "TRH", "LSE", "RSE", "LEH", "REH", "HH ", "LHK", "RHK", "LKF", "RKF"};
  //int wdt = 10;
  for (int i = 0; i < 15; i++) {
    txs << left << setw(6) << acBones[i]
    <<left<< setw(13) << setprecision(3) << fixed << showpos << distances[bonesIndex[0][i]][bonesIndex[1][i]]
    <<left<< setw(13) << setprecision(3) << fixed <<vmean[i]
    <<left<< setw(13) << setprecision(2) << scientific <<stDev[i]
    <<left<< setw(13) << setprecision(2) << scientific <<(distances[bonesIndex[0][i]][bonesIndex[1][i]] - vmean[i]) / stDev[i];
    
    //std::cout<<"stdev  "<<stDev[i]<<std::endl;
    tabela1.push_back(txs.str());
    txs.str("");
  }
  
  
  
  int pY1 = 50;
  int pX1 = 690;
  int d1 = 0;
  std::ostringstream columnsLabels;
  
  columnsLabels << setw(13) << left << setfill(' ')<< "Osso"<< setw(10) << "Size" << setw(10) << "Media"<< setw(10)<< "Desvio" << setw(10) << "Z-score";
  
  putText(background, columnsLabels.str(), Point(pX1, pY1 - 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 1, 8, false);
  for (int i = 0; i < 15; i++) {
    putText(background, tabela1.at(i), Point(pX1, pY1 + d1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 1, 8, false);
    //std::cout << tabela1.at(i) << std::endl;
    d1 += 20;
  }
  
  std::vector<String> t = LOI.mahal(distances, bonesIndex);
  //std::cout << t << std::endl;
  
  int st = t.size();
  int p = 0;
  if (st > 3) {
    st = 3;
  }
  int pY2 = 400;
  int pX2 = 660;
  putText(background, "Individuos mais parecidos", Point(pX2, pY2 - 30), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 1, 8, false);
  
  
  for (int i = 0; i < st; i++) {
    putText(background, t.at(i), Point(pX2, pY2 + p), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 1, 8, false);
    //std::cout << t.at(i) << std::endl;
    p += 20;
  }
  
  
  
  int ps[5] = {2, 3, 6, 9, 12};
  std::vector < std::vector< float > > points(5);
  for (int i = 0; i < 5; i++) {
    points.at(i).push_back(msg->position[ps[i]].x);
    points.at(i).push_back(msg->position[ps[i]].y);
    points.at(i).push_back(msg->position[ps[i]].z);
  }
  
  
  Stat st2;
  std::vector< float > plane = st2.fitPlane(points);
  
  std::vector<float> xyplane(4);
  xyplane.at(0) = 0;
  xyplane.at(1) = 0;
  xyplane.at(2) = 1;
  xyplane.at(3) = 0;
  float angle = st2.anglePlanes(plane, xyplane);
  std::ostringstream angleString;
  angleString << angle;
  
  
  putText(background, angleString.str(), Point(40, 40), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 1, 8, false);
  angleString.str("");
  if (angle >= 45) {
    std::cout << "Deve-se virar o corpo para frente e permanecer ereto" << std::endl;
    putText(background, "Deve-se virar o corpo para frente e permanecer ereto", Point(50, 50), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 1, 8, false);
  }
  
  using namespace nsJoints;
  //ponto da normal + torso
  
  float normal3dpoint[3] = {msg->position[TORSO].x + plane.at(0), (-1) * msg->position[TORSO].y + plane.at(1), msg->position[TORSO].z + plane.at(2)};
  float normal3dpoint2[3] = {msg->position[TORSO].x + (-1) * plane.at(0), (-1) * msg->position[TORSO].y + (-1) * plane.at(1), msg->position[TORSO].z + (-1) * plane.at(2)};
  
  //projeção em 2d com perspectiva de normal3dpoint
  Point normal2dpoint = Point(centerx + (fx * normal3dpoint[0]) / normal3dpoint[2], centery + (fy * normal3dpoint[1]) / normal3dpoint[2]);
  Point normal2dpoint2 = Point(centerx + (fx * normal3dpoint2[0]) / normal3dpoint2[2], centery + (fy * normal3dpoint2[1]) / normal3dpoint2[2]);
  
  //float l=sqrt(plane.at(0)*plane.at(0)+plane.at(1)*plane.at(1)+plane.at(2)*plane.at(2));
  //float nv[3]={msg->position[TORSO].x+plane.at(0),msg->position[TORSO].y+plane.at(1),msg->position[TORSO].z+plane.at(2)};
  //ponto do torso (2d com perspectiva)
  Point Torso = Point(centerx + (fx * msg->position[TORSO].x) / msg->position[TORSO].z, centery + (-1) * (fy * msg->position[TORSO].y) / msg->position[TORSO].z);
  //ponto que representa o vetor normal(2d com perspectiva)
  //Point normal = Point(centerx+ (fx*nv[0])/nv[2],centery+(fy*nv[1])/nv[2]);
  Scalar color1;
  Scalar color2;
  color1 = Scalar(0, 100, 0);
  color2 = Scalar(124, 252, 0);
  
  line(background, Torso, normal2dpoint, color1, 3, 8, 0);
  line(background, Torso, normal2dpoint2, color2, 3, 8, 0);
  
  //imshow("Display", background);
  //return waitKey(1);
  
  
  
  //imshow("Display2", camera);
  //static char act = 'P'; 
  
  /*
   * 
   *        if (wK == 'R' || wK == 'r') {
   *            act = 'R';
   }
   
   if (wK == 'E' || wK == 'e') {
    int iD;
    std::cout << "What individual would you like to delete? Type in a number:" << std::endl;
    std::cin >> iD;
    
    LOI.Individuals.erase(LOI.Individuals.begin() + iD);
  
   }
   
   LOI.collectSamples(act, distances, 15, bonesIndex, 100);
  LOI.saveToFile("/home/dalton/ros_workspace/distances/DataSets/DataSet.xml");
  
  std::cout << "wK = " << wK << std::endl;
  std::cout << "act = " << act << std::endl;
  */
   }
   
   Display::Display() {
     act = 'P';
   }
   
   char Display::showWindow(bool pi, bool ps) {
     using namespace cv;
     
     
     if(!pi && !ps){
       std::cout << "sem imagem sem esqueleto" << std::endl;
       background = Mat::zeros(480, 640 + 600, CV_8UC3);
       
     } else if (pi && !ps){
       std::cout << "imagem sem esqueleto" << std::endl;
       background = Mat::zeros(480, 640 + 600, CV_8UC3);
       cv::Rect roi(cv::Point(0, 0), cv::Size(640, 480));
       //std::cout << "26a" << std::endl;
       //cv::Rect roi( 0, 0 ,  200, 300);
       //cv::Mat destinationROI = background(roi);
       Mat destinationROI(background, roi);
       //std::cout << "29" << std::endl;
       camera.copyTo(destinationROI);
       //std::cout << "30" << std::endl;
       
     } else if(!pi && ps){
       std::cout << "esqueleto sem imagem" << std::endl;
       cv::Rect roi(cv::Point(0, 0), cv::Size(640, 480));
       //std::cout << "26b" << std::endl;
       //cv::Rect roi( 0, 0 ,  200, 300);
       //cv::Mat destinationROI = background(roi);
       Mat destinationROI(background, roi);
      // std::cout << "29" << std::endl;
       camera.copyTo(destinationROI);
       //std::cout << "30" << std::endl;
       
     } else {
       std::cout << "com imagem e esqueleto" << std::endl;
       background = Mat::zeros(480, 640 + 600, CV_8UC3);
       cv::Rect roi(cv::Point(0, 0), cv::Size(640, 480));
       //std::cout << "26c" << std::endl;
       //cv::Rect roi( 0, 0 ,  200, 300);
       //cv::Mat destinationROI = background(roi);
       Mat destinationROI(background, roi);
       //std::cout << "29" << std::endl;
       camera.copyTo(destinationROI);
       //std::cout << "30" << std::endl;
     }
     //std::cout << "display" << std::endl;
     imshow("Display", background);
     //std::cout << "display2" << std::endl;
     return waitKey(1);
     //std::cout << "display3" << std::endl;
   }
   